﻿#include "imu.h"
#include "ceres/ceres.h"
#include <cstdio>
#include <QDebug>
#include <QtMath>
#pragma execution_character_set("utf-8")
void ZVD::set_norm_buf_size(int var_size) {
    acc_norm_size = var_size;
    acc_norm_var_buf->resize(acc_norm_size);
}
void ZVD::set_norm_threshold(double up, double bottom)
{
    _acc_up = up;
    _acc_bottom=bottom;
}
void ZVD::set_norm_var_limit(double up) {
    _acc_var_limit = up;
}
void ZVD::set_duration_in_out(int in, int out) {
    duration_in = in; duration_out = out;
}
bool ZVD::update(Eigen::Vector3d acc) {
//    qDebug()<<"sum::"<<acc_norm_var_buf->sum()<<" "<<acc_norm_var_buf->get_size();
    acc_norm = acc.norm();
    acc_norm_var_buf->add(acc_norm);
    double acc_norm_var = acc_norm_var_buf->var();
//    qDebug()<< " "<<acc_norm<<" "<<acc_norm_var<<" "<<acc_norm_var_buf.mean()<<" "<<acc_norm_var_buf.sum();
//    qDebug()<< " "<<acc_norm<<" "<<acc_norm_var;
//    qDebug()<< "::"<<count<<" ";
    if(is_stationary == false){
        if(acc_norm <= _acc_up && acc_norm >= _acc_bottom && acc_norm_var < _acc_var_limit){

            // 满足条件
            if(++count >= duration_in){
                // 正式进入静止
                is_stationary = true;
                count = duration_out;
            }
        } else{
            // 不满足条件 清空
            count = 0;
        }
    }else{
        // 正式进入
        if(acc_norm > _acc_up || acc_norm < _acc_bottom || acc_norm_var >= _acc_var_limit){
            // 动了
            if(--count == 0){
                is_stationary = false;
            }else{

            }
        }else{
            // 没有连续动
            count = duration_out;
        }
    }
    return is_stationary;
}

struct RotationAxisError
{
    RotationAxisError( const Eigen::Vector3d &a) :
        acc_(a){}

    template <typename T>
    bool operator() ( const T* const params, T* residuals ) const
    {
        Eigen::Matrix< T, 3 , 1> v;
        for(int i = 0; i<3;++i){
            v[i] = acc_[i] - params[i];
        }
        // 园和半径
        residuals[0] = v.norm()-params[3];
        return true;
    }

    static ceres::CostFunction* Create ( const Eigen::Vector3d &a )
    {
        return ( new ceres::AutoDiffCostFunction< RotationAxisError, 1, 4 > (
                     new RotationAxisError(a) ) );
    }

    Eigen::Vector3d acc_;
};

IMU::IMU() {
    zvd = new ZVD(10);
    ekf = new EKF_A();

    ekf->init();
    rotation_axis[0] = 1;
    rotation_axis[1] = 0;
    rotation_axis[2] = 0;
//    acc_norm_var_buf.resize(acc_norm_size);
}
IMU::~IMU() {
    delete zvd;
    delete ekf;
}
void IMU::set_acc_norm_threshold(double up, double bottom)
{
    zvd->set_norm_threshold(up,bottom);
}
void IMU::set_acc_norm_var_limit(double up) {
    zvd->set_norm_var_limit(up);
}

Vector3d & IMU::get_rotation_axis(){
//    if(is_rotation_axis_get){
//        return rotation_axis;
//    }else{
//        Vector3d a(1,0,0);
//        return a;
//    }
    return rotation_axis;
}
QString IMU::calculate_rotation_axis()
{
    QString res = "fail";
    ZVD zvd_tmp(10);
    std::vector<std::vector<Eigen::Vector3d>> static_buf;
    std::vector<Eigen::Vector3d> static_v;
    bool flag = false;
     qDebug()<< acc_buf.size();
    for(auto &a : acc_buf){
        if(zvd_tmp.update(a)){
            flag = true;
//            qDebug()<<1;
            // 进入开始存
            static_v.push_back(a);
        }else{
//            qDebug()<<0;
            if(flag == true){
                // 从静态退出
//                qDebug()<< static_v.size();
                if(static_v.size()>10){
                    static_buf.push_back(static_v);
                }
                // 数量不够，
                static_v.clear();
                flag = false;
            }
        }
    }
    qDebug()<< static_buf.size();
    if(static_buf.size() > 3){
        // 叉乘计算旋转轴
        std::vector<Eigen::Vector3d> acc_mean_buf; // 静止平均缓存
        for( int i = 0; i < static_buf.size(); i++)
        {
            auto a_size = static_buf[i].size();
            if(a_size >= 50){
                Eigen::Vector3d _sum{0,0,0};
                for(auto& a:static_buf[i]){
                    _sum +=a;
                }
                acc_mean_buf.push_back(_sum/a_size);
            }
        }

        std::vector<Eigen::Vector3d> acc_mean_buf_next;
        qDebug()<<acc_mean_buf.size();
        acc_mean_buf_next.push_back(acc_mean_buf[0]);

        for( int i = 1; i < acc_mean_buf.size(); i++){
            Eigen::Vector3d diff = acc_mean_buf[i]-acc_mean_buf[i-1];
            if(diff.norm() > M_PI/9){
                // 夹角大于 30°
                acc_mean_buf_next.push_back(acc_mean_buf[i]);
            }
            if(acc_mean_buf_next.size() >= 3)
                break; // 跳出去在外面计算
        }
        Eigen::Vector3d rotation_axis_first;
        if(acc_mean_buf_next.size() >= 3){
            // 有三个
            Eigen::Vector3d first = acc_mean_buf_next[1] - acc_mean_buf_next[0];
            Eigen::Vector3d second = acc_mean_buf_next[2] - acc_mean_buf_next[1];
            rotation_axis_first = first.cross(second);
            if(rotation_axis_first.x() < 0 ){
                rotation_axis_first = -rotation_axis_first;
            }
            rotation_axis = rotation_axis_first/rotation_axis_first.norm();
            is_rotation_axis_get = true;
            res = "Success!!";
        }else{
            // 没有三个
            res = "失败，旋转间隔角度太小或少于三次！";
            qDebug()<<acc_mean_buf_next.size();
            acc_buf.clear();
        }

        #if 0
        // 优化计算旋转轴 好像不太好用
        std::vector<double> param(4);
        param[0] = 1;
        param[3] = 1;
        ceres::Problem problem;
        for( int i = 0; i < static_buf.size(); i++)
        {
            for(auto& a:static_buf[i]){
                ceres::CostFunction* cost_function =
                        RotationAxisError::Create (a);

                problem.AddResidualBlock ( cost_function, NULL /* squared loss */, param.data());
            }
        }
        ceres::Solver::Options options;
        options.linear_solver_type = ceres::DENSE_QR;
        options.minimizer_progress_to_stdout = true;
        ceres::Solver::Summary summary;
        ceres::Solve (options, &problem, &summary );
        std::cout<<summary.FullReport()<<std::endl;

        std::cout<<"residual "<<summary.final_cost<<std::endl;
        if(summary.final_cost < 0.02){
            for(int i = 0;i<3;++i){
                rotation_axis[i] = param[i];
            }
            rotation_axis = rotation_axis/rotation_axis.norm();
            std::cout<<rotation_axis<<std::endl;
            is_rotation_axis_get = true;
            res = "Success!!";
        }else{
            res = "失败，残差太大！";
            acc_buf.clear();
        }
        #endif
    }else{
        is_rotation_axis_get = false;
        qDebug()<<"error: data too small\n";
        res = "失败，静止区间太少！";
    }

    return res;

}

QString IMU::set_rotation_starting_point()
{
    QString res;
    if(init_acc_buf.size()<100){
        res = "数据太少,请保持静止1秒钟！";
        is_init_acc_ = false;
    }else{
        Eigen::Vector3d sum(0,0,0);
        for_each(init_acc_buf.begin(),init_acc_buf.end(),[&](const Eigen::Vector3d &a){
            sum+=a;
        });
        init_acc = sum/sum.norm();
        res = "旋转起始点设定完成！";
        is_init_acc_ = true;
        ekf->reset();
    }
    return res;
}

IMU::STATE IMU::update_data( Eigen::Vector3d& a) {
    acc = a;

    IMU::STATE sta;
    bool is_static = zvd->update(acc);
    sta = is_static? STATION : MOVING;

    if(is_rotation_axis_get == false){
        acc_buf.push_back(acc); // 计算旋转轴缓存
    }else{
        
        if(is_static == false){
            // 不静止就清零
            init_acc_buf.clear();
//            qDebug()<<0;
        }else{
            // 静止的时候存数据
            if(init_acc_buf.size() <3000){
                // 记录数据不要太多
                init_acc_buf.push_back(acc);
            }else{
                init_acc_buf.erase(init_acc_buf.begin(),init_acc_buf.begin()+1000);
            }
        }
        if(is_init_first_acc() == false){
            // 初始化起始点的函数 为set_rotation_starting_point()
            // 由外部调用
        }else{
            if(!ekf->is_init()){// 计算初始化加速度计
                ekf->init(rotation_axis,init_acc,0.0002,Matrix3d::Identity()*0.0063);

            }else{
                if(is_static){
                    // 静止 保持kf更新

                }else{
                    // 动 不更新

                }
                ekf->update(acc);
                angle = ekf->get_angle()/M_PI*180;
            }
        }
    }

    return sta;
}

double IMU::get_angle()
{
    return angle;
}

void IMU::importAsciiData ( const char *filename)
{
    acc_buf.clear();
    init_acc_buf.clear();
    string line;
    ifstream infile;
    double ts, d[3];
    infile.open ( filename );
    if ( infile.is_open() )
    {
        char format[266];
//        switch ( type )
//        {
//        case DATASET_COMMA_SEPARATED:
//            sprintf ( format,"%%lf, %%lf, %%lf, %%lf" );
//            break;
//        case DATASET_SPACE_SEPARATED:
//        default:
            sprintf ( format,"%%lf %%lf %%lf %%lf" );
//            break;
//        }

        int l = 0;
        while ( getline ( infile,line ) )
        {
            int res = sscanf ( line.data(), format, &ts, &d[0], &d[1], &d[2] );
            if ( res != 4 )
            {
                cout<<"importAsciiData(): error importing data in line "<<l<<", exit"<<endl;
            }
            else
            {
//                ts /= unit;
//                Eigen::Vector3d
//                acc_buf.push_back ((Eigen::Vector3d ()<<d[0],d[1],d[2]).finished());
                update_data((Eigen::Vector3d ()<<d[0],d[1],d[2]).finished());
            }

            l++;
        }
        infile.close();

    }
    std::cout <<"sdfasd"<< acc_buf.size()<<std::endl;
}
